/***************************************************************************************************************************
* formation_avoidance.h
*
* Author: SFL
*
* Update Time: 2021.05.18

* Introduction:  用来实现队内避障
            1.通过自己当前位置和其他无人机相对自己的位置生成自己应该运动的方向。
            2.当超过一定范围的情况下不做躲避。
            3.目前先只通过位置生成速度方向，因为数传暂时不能传速度回来。如果以后有机会可以把所有无人机当前速度也考虑进来。
            4.鉴于普通GPS误差较大，建议测试避障阈值给大一点。
***************************************************************************************************************************/
#ifndef FORMATION_AVOIDANCE_H
#define FORMATION_AVOIDANCE_H

#include <global2local.h>
#include <wrzf_pkg/DroneState.h>
#include <wrzf_pkg/UAVPositionDetectionInfo.h>
#include <px4_command_utils.h>
double k_a = 4.0;
double k_b = 2.0;
double d_min = 3;
double d_r = 5;
double d_a = 6;
double d_max = 10;
void generate_avoidance_direction(Eigen::Vector3d origin_lla, vector<Eigen::Vector3d> all_uavs_lla_, 
                                    wrzf_pkg::DroneState _DroneState, Eigen::Vector3d& tar_direction,
                                    int QuadID)
{
    Eigen::Vector3d cur_position = Eigen::Vector3d(_DroneState.position[0],_DroneState.position[1],_DroneState.position[2]);
    Eigen::Vector3d delta_position;
    Eigen::Vector3d uav_enu;
    
    // tar_direction = Eigen::Vector3d(0.0,0.0,0.0);
    for (int i = 0; i < all_uavs_lla_.size(); i++)
    {
        if (i == QuadID)
        {
            tar_direction += Eigen::Vector3d(0.0,0.0,0.0);
        }
        else if (i == QuadID - 1 || i == QuadID +1)
        {
            uav_enu = lla_to_enu(origin_lla, all_uavs_lla_[i]);
            delta_position = cur_position - uav_enu;
            if (delta_position.norm() < d_min)
            {
                tar_direction += 10 * (delta_position / delta_position.norm());
            }
            else if(delta_position.norm() >= d_min && delta_position.norm() < d_r)
            {
                tar_direction += k_b * (1 / (delta_position.norm() - (d_min - 0.2)) - 
                                    1 / (d_r - (d_min - 0.2))) * 
                                    (delta_position / delta_position.norm());
            }
            // else if(delta_position.norm() >= d_a && delta_position.norm() < d_max)//这部分为防止出现异常再考虑一下是否去掉
            // {
            //     tar_direction += k_a * (1 / (d_max + 0.4 - d_a) - 
            //                         (1 / (d_max + 0.4 - delta_position.norm()))) * 
            //                         (delta_position / delta_position.norm());
            // }
            else
            {
                tar_direction += Eigen::Vector3d(0.0,0.0,0.0);
            }
        }
        else
        {
            uav_enu = lla_to_enu(origin_lla, all_uavs_lla_[i]);
            delta_position = cur_position - uav_enu;
            if (delta_position.norm() < d_min)
            {
                tar_direction += 10 * (delta_position / delta_position.norm());
            }
            else if(delta_position.norm() >= d_min && delta_position.norm() < d_r)
            {
                tar_direction += k_b * (1 / (delta_position.norm() - (d_min - 0.2)) - 
                                    1 / (d_r - (d_min - 0.2))) * 
                                    (delta_position / delta_position.norm());
            }
            else
            {
                tar_direction += Eigen::Vector3d(0.0,0.0,0.0);
            }
        }
    }
    
}

void generate_avoidance_direction_by_vision(wrzf_pkg::UAVPositionDetectionInfo _UAV_Detection_Info, 
                                            wrzf_pkg::DroneState _DroneState,
                                            Eigen::Vector3d& tar_direction)
{
    // Eigen::Vector3d delta_position_body = Eigen::Vector3d(_UAV_Detection_Info.position[2],
    //                                                     -_UAV_Detection_Info.position[0],
    //                                                     -_UAV_Detection_Info.position[1]);
    float d_pos_body[2];        //the delta xy position in Body Frame
    float d_pos_enu[2];         //the delta xy position in enu Frame (The origin point is the drone)
    d_pos_body[0] = _UAV_Detection_Info.position[2];
    d_pos_body[1] = -_UAV_Detection_Info.position[0];
    px4_command_utils::rotation_yaw(_DroneState.attitude[2], d_pos_body, d_pos_enu);
    Eigen::Vector3d delta_position = Eigen::Vector3d(d_pos_enu[0],
                                                    d_pos_enu[1],
                                                    -_UAV_Detection_Info.position[1]);
    if (delta_position.norm() < d_min)
    {
        tar_direction = 10 * (delta_position / delta_position.norm());
    }
    else if(delta_position.norm() >= d_min && delta_position.norm() < d_r)
    {
        tar_direction = k_b * (1 / (delta_position.norm() - (d_min - 0.2)) - 
                            1 / (d_r - (d_min - 0.2))) * 
                            (delta_position / delta_position.norm());
    }
    else
    {
        tar_direction = Eigen::Vector3d(0.0,0.0,0.0);
    }
}

#endif //FORMATION_AVOIDANCE_H